package RVO;

import javax.swing.*;
import java.awt.*;
import java.awt.event.ActionEvent;
import java.awt.event.ActionListener;
import java.awt.event.MouseAdapter;
import java.awt.event.MouseEvent;

//创建指定agent往指定目标移动, 用于观测算法是否正确
class ORCAVisualMove extends JPanel implements ActionListener {
    private Simulator simulator;
    private Timer timer;
    private int frameCount;
    private Vector2 mousePoint = new Vector2(0, 0);

    public ORCAVisualMove() {
        simulator = new Simulator();
        frameCount = 0;

        addMouseMotionListener(new MouseAdapter() {
            @Override
            public void mouseMoved(MouseEvent e) {
                mousePoint.x_ = e.getPoint().x;
                mousePoint.y_ = e.getPoint().y;
                /*
                System.out.println("鼠标坐标: (" + mousePoint.x + ", " + mousePoint.y +
                        ") | 窗口大小: " + getWidth() + " x " + getHeight());
                 */
                repaint(); // 如果需要重绘显示坐标
            }
        });

        addMouseListener(new MouseAdapter() {

            @Override
            public void mousePressed(MouseEvent e) {
                if (SwingUtilities.isLeftMouseButton(e)) {
                    int id = simulator.addAgent(new Vector2(e.getPoint().x, e.getPoint().y));
                    simulator.agents_.get(id).timeStep_ = 2f;
                    repaint();
                }
            }
        });

        timer = new Timer(33, this); // 更快的更新频率
        timer.start();

        setPreferredSize(new Dimension(500, 500));
        setBackground(Color.WHITE);

        /* Specify the global time step of the simulation. */
        simulator.setTimeStep(2f);

        /*
         * Specify the default parameters for agents that are subsequently
         * added.
         */
        simulator.setAgentDefaults(45.0f, 10, 5.0f, 5.0f,
                10f, 2.0f, new Vector2(0.0f, 0.0f));

        /*
         * Add agents, specifying their start position, and store their
         * goals on the opposite side of the environment.
         */
    }

    @Override
    protected void paintComponent(Graphics g) {
        super.paintComponent(g);
        Graphics2D g2d = (Graphics2D) g;
        g2d.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);

        // 绘制信息
        g2d.setColor(Color.BLACK);
        g2d.setFont(new Font("Arial", Font.PLAIN, 14));
        g2d.drawString("ORCA避障算法演示 - ", 10, 20);
        g2d.drawString("帧数: " + frameCount, 10, 40);

        for (int i = 0; i < simulator.getNumAgents(); ++ i) {
            Agent agent = simulator.agents_.get(i);
            g2d.fillOval(
                    (int) (agent.position_.x_ - agent.radius_),
                    (int) (agent.position_.y_ - agent.radius_),
                    (int) (agent.radius_ * 2),
                    (int) (agent.radius_ * 2)
            );
        }
    }

    public void setPreferredVelocities() {
        /*
         * Set the preferred velocity to be a vector of unit magnitude
         * (speed) in the direction of the goal.
         */
        for (int i = 0; i < simulator.getNumAgents(); ++i) {
            Vector2 goalVector = Vector2.subtract(mousePoint, simulator.getAgentPosition(i));
            if (goalVector.x_ * goalVector.x_ + goalVector.y_ * goalVector.y_ > 1.0f) {
                goalVector = Vector2.divide(goalVector, ((float) Math.sqrt(goalVector.x_ * goalVector.x_
                        + goalVector.y_ * goalVector.y_)));
            }

            //System.out.printf("%s 目标%s  移动向量[%f,%f]\n", Simulator.Instance.getAgentPosition(i),
                    //mousePoint, goalVector.x_, goalVector.y_);
            simulator.setAgentPrefVelocity(i, goalVector);
        }
    }

    @Override
    public void actionPerformed(ActionEvent e) {
        frameCount++;
        setPreferredVelocities();
        simulator.doStep();
        repaint();
    }
}

// 主类
public class Demo2 {
    public static void main(String[] args) {
        SwingUtilities.invokeLater(() -> {
            JFrame frame = new JFrame("ORCA避障算法演示");
            frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
            frame.add(new ORCAVisualMove());
            frame.pack();
            frame.setLocationRelativeTo(null);
            frame.setVisible(true);
        });
    }
}